perm filename TMP[CRE,BGB] blob
sn#050733 filedate 1973-10-20 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00008 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 BEGIN "TEST"
C00004 00003 α IRON TRIANGLE - CAMERA LOCUS SOLVER
C00006 00004 SUBR CAMINIT
C00009 00005 SUBR TRANSFORM
C00011 00006 SUBR SHOW
C00013 00007 SUBR MODIFY
C00016 00008 α MAIN LOOP
C00017 ENDMK
C⊗;
BEGIN "TEST"
REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
REQUIRE "DPYIII[SYS,BGB]" SOURCE_FILE;
REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
SAFE ITG ARRAY DPYBUF[0:999];
α CAMERA;
REAL PAN,TILT,SWING;
REAL TRNDEL,ROTDEL,DELTA;
REAL CX,CY,CZ;
REAL IX,IY,IZ;
REAL JX,JY,JZ;
REAL KX,KY,KZ;
REAL SX,SY,FOCAL,MAG; α RASTER SCALES;
REAL LDX,LDY,PDX,PDY; α LOGICAL AND PHYSICAL RASTERS;
REAL XPA,YPA; α PRINCIPLE AXIS LOCUS IN PP;
REAL RPA,CPA; α PRINCIPLE AXIS LOCUS IN RC;
α THREE INCH CUBE;
PRELOAD_WITH 0,3,3,3,0,0;SAFE REAL ARRAY XWC[1:6];
PRELOAD_WITH 0,0,0,3,3,3;SAFE REAL ARRAY YWC[1:6];
PRELOAD_WITH 0,0,3,3,3,0;SAFE REAL ARRAY ZWC[1:6];
PRELOAD_WITH '216,'176,'122,'105,'124,'200;SAFE ITG ARRAY ROW[1:6];
PRELOAD_WITH '220,'247,'251,'210,'160,'160;SAFE ITG ARRAY COL[1:6];
SAFE ITG ARRAY PREDROW[1:6];
SAFE ITG ARRAY PREDCOL[1:6];
SAFE REAL ARRAY XPP,YPP,ZPP,XDC,YDC[1:6];
SAFE REAL ARRAY XXPP,YYPP,XXDC,YYDC[1:6];
SAFE REAL ARRAY XCC,YCC,ZCC[1:6];
α IRON TRIANGLE - CAMERA LOCUS SOLVER;
REAL ARRAY P1,P2,P3,COSANG[1:3],V[1:10,1:3];
REQUIRE "LS1V3P.REL" LOAD_MODULE;
EXTERNAL ITG PROCEDURE LS1V3P(REAL ARRAY V,P1,P2,P3,COSANG);
REAL SUBR DOTVEC(ITG I,J);
BEGIN "DOTVEC"
REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2,ZCOS;
X1 ← XCC[I]; Y1 ← YCC[I]; Z1 ← ZCC[I];
X2 ← XCC[J]; Y2 ← YCC[J]; Z2 ← ZCC[J];
R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);
R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
ZCOS←(X1*X2 + Y1*Y2 + Z1*Z2) / (R1*R2);
RETURN(ZCOS);
END "DOTVEC";
SUBR LSCAM (ITG I,K,J);
BEGIN "LSCAM"
ITG M,N;
α IRON TRIANGLE - KNOWN WORLD LOCI;
P1[1]←XWC[I]; P2[1]←XWC[J]; P3[1]←XWC[K];
P1[2]←YWC[I]; P2[2]←YWC[J]; P3[2]←YWC[K];
P1[3]←ZWC[I]; P2[3]←ZWC[J]; P3[3]←ZWC[K];
α IRON TRIPOD - KNOW ANGLES BETWEEN CAMERA RAYS;
COSANG[1] ← DOTVEC(J,K);
COSANG[2] ← DOTVEC(I,K);
COSANG[3] ← DOTVEC(I,J);
α THROW THE SHIT AT THE FAN;
M ← LS1V3P(V,P1,P2,P3,COSANG);
OUTSTR(9&CVS(M)&" CAMERA SOLUTIONS."&↓);
FOR N←1 TIL M DO
OUTSTR(9&CVG(V[N,1])&9&CVG(V[N,2])&9&CVG(V[N,3])&↓);
INCHRW;
END "LSCAM";
SUBR CAMINIT;
BEGIN "CAMINIT"
REAL RR;
REAL C_PAN,S_PAN,C_TILT,S_TILT,C_SWING,S_SWING;
IF CX=0 THEN BEGIN
RPA ← 108; CPA ← 144;
CX ← -30; CY ← -24; CZ ← +22;
PAN ←-52*π/180;
TILT ← 62*π/180;
SWING ← 0;
MAG ← 32/9;
FOCAL ← 25000;
PDX ← 42.8; LDX ← 288;
PDY ← 35.8; LDY ← 216;
END;
C_PAN ← COS(PAN); S_PAN ← SIN(PAN);
C_TILT ← COS(TILT); S_TILT ← SIN(TILT);
C_SWING ← COS(SWING); S_SWING ← SIN(SWING);
IX ← C_PAN*C_SWING - S_PAN*C_TILT*S_SWING;
IY ← S_PAN*C_SWING + C_PAN*C_TILT*S_SWING;
IZ ← S_TILT*S_SWING;
JX ← -C_PAN*S_SWING - S_PAN*C_TILT*C_SWING;
JY ← -S_PAN*S_SWING + C_PAN*C_TILT*C_SWING;
JZ ← S_TILT*C_SWING;
KX ← S_PAN*S_TILT;
KY ← -C_PAN*S_TILT;
KZ ← C_TILT;
END "CAMINIT";
SUBR TRANSFORM;
BEGIN "TRANSFORM"
ITG I;
FOR I←1 TIL 6 DO
BEGIN
REAL X,Y,Z;
α WC → CC WORLD LOCII;
X ← XWC[I] - CX;
Y ← YWC[I] - CY;
Z ← ZWC[I] - CZ;
XCC[I] ← X*IX + Y*IY + Z*IZ;
YCC[I] ← X*JX + Y*JY + Z*JZ;
ZCC[I] ← X*KX + Y*KY + Z*KZ;
α CC → PP;
SX ← -FOCAL/PDX;
SY ← -FOCAL/PDY;
XPP[I] ← SX * XCC[I] / ZCC[I];
YPP[I] ← SY * YCC[I] / ZCC[I];
α PP → RC;
PREDROW[I] ← RPA - YPP[I];
PREDCOL[I] ← XPP[I] + CPA;
α RC → PP PERCEIVED;
XPP[I] ← COL[I] - CPA;
YPP[I] ← RPA - ROW[I];
α PP → CC IMAGE PLANE LOCII;
XCC[I] ← -XPP[I]*FOCAL/SX;
YCC[I] ← -YPP[I]*FOCAL/SY;
ZCC[I] ← -FOCAL;
α PP → DC;
XXDC[I] ← MAG * (COL[I]-144);
YYDC[I] ← MAG * (108-ROW[I]);
XDC[I] ← MAG * (PREDCOL[I]-144);
YDC[I] ← MAG * (108-PREDROW[I]);
END;
END "TRANSFORM";
SUBR SHOW;
BEGIN "SHOW"
ITG I;
DPYSET(DPYBUF);AIVECT(-511,-384);
AVECT(+511,-384);AVECT(+511,+384);AVECT(-511,+384);AVECT(-511,-384);
AIVECT(400,480);DPYSST("PAN "&CVS(PAN*180/π));
AIVECT(400,455);DPYSST("TILT "&CVS(TILT*180/π));
AIVECT(400,430);DPYSST("SWING "&CVS(SWING*180/π));
AIVECT(400,405);DPYSST("ROTDEL "&CVS(ROTDEL*180/π));
AIVECT(-400,380);DPYSST(CVG(IX)&9&CVG(IY)&9&CVG(IZ));
AIVECT(-400,350);DPYSST(CVG(JX)&9&CVG(JY)&9&CVG(JZ));
AIVECT(-400,320);DPYSST(CVG(KX)&9&CVG(KY)&9&CVG(KZ));
AIVECT(400,375);DPYSST("CX = "&CVG(CX));
AIVECT(400,350);DPYSST("CY = "&CVG(CY));
AIVECT(400,325);DPYSST("CZ = "&CVG(CZ));
AIVECT(400,300);DPYSST("TRNDEL "&CVG(TRNDEL));
AIVECT(400,250);DPYSST("PDX = "&CVG(PDX));
AIVECT(400,225);DPYSST("PDY = "&CVG(PDY));
AIVECT(400,200);DPYSST("FOCAL = "&CVG(FOCAL));
AIVECT(400,150);DPYSST("RPA = "&CVG(RPA));
AIVECT(400,125);DPYSST("CPA = "&CVG(CPA));
AIVECT(XDC[6],YDC[6]);FOR I←1 TIL 6 DO AVECT(XDC[I],YDC[I]);
AIVECT(XXDC[6],YYDC[6]);FOR I←1 TIL 6 DO AVECT(XXDC[I],YYDC[I]);
DPYOUT(1);
END "SHOW";
SUBR MODIFY;
BEGIN "MODIFY"
ITG CHR,CTRL,META;
CHR ← INCHRW;
CTRL ← CHR LAND '200;
META ← CHR LAND '400;
CHR ← CHR LAND '177;
IF (CTRL∧¬META) THEN
BEGIN
IF CHR="/" THEN ROTDEL ← ROTDEL/2 ELSE
IF CHR="\" THEN ROTDEL ← ROTDEL*2 ELSE
IF CHR=";" THEN TILT ← TILT-ROTDEL ELSE
IF CHR=":" THEN TILT ← TILT+ROTDEL ELSE
IF CHR="(" THEN PAN ← PAN -ROTDEL ELSE
IF CHR=")" THEN PAN ← PAN +ROTDEL ELSE
IF CHR="-" THEN SWING ← SWING - ROTDEL ELSE
IF CHR="*" THEN SWING ← SWING + ROTDEL;
END;
IF ¬(CTRL∨META) THEN
BEGIN
IF CHR="/" THEN TRNDEL ← TRNDEL/2 ELSE
IF CHR="\" THEN TRNDEL ← TRNDEL*2 ELSE
IF CHR=";" THEN CX ← CX-TRNDEL ELSE
IF CHR=":" THEN CX ← CX+TRNDEL ELSE
IF CHR="(" THEN CY ← CY-TRNDEL ELSE
IF CHR=")" THEN CY ← CY+TRNDEL ELSE
IF CHR="-" THEN CZ ← CZ-TRNDEL ELSE
IF CHR="*" THEN CZ ← CZ+TRNDEL;
END;
IF CTRL∧META THEN
BEGIN
IF CHR=";" THEN CPA ← CPA-TRNDEL ELSE
IF CHR=":" THEN CPA ← CPA+TRNDEL ELSE
IF CHR="(" THEN RPA ← RPA+TRNDEL ELSE
IF CHR=")" THEN RPA ← RPA-TRNDEL;
END;
IF META∧¬CTRL THEN
BEGIN
IF CHR=";" THEN PDX ← PDX-TRNDEL ELSE
IF CHR=":" THEN PDX ← PDX+TRNDEL ELSE
IF CHR="(" THEN PDY ← PDY-TRNDEL ELSE
IF CHR=")" THEN PDY ← PDY+TRNDEL ELSE
IF CHR="-" THEN FOCAL ← FOCAL-TRNDEL*1000 ELSE
IF CHR="*" THEN FOCAL ← FOCAL+TRNDEL*1000;
END;
END "MODIFY";
α MAIN LOOP;
TRNDEL ← 2;
ROTDEL ← π/2;
WHILE TRUE DO
BEGIN
CAMINIT;
TRANSFORM;
SHOW;
MODIFY;
END;
OUTSTR("*");INCHRW;
LSCAM(1,2,3);
LSCAM(1,2,4);
LSCAM(1,2,5);
LSCAM(1,2,6);
LSCAM(2,3,4);
LSCAM(2,3,5);
LSCAM(2,3,6);
LSCAM(3,4,5);
LSCAM(3,4,6);
LSCAM(4,5,6);
WHILE TRUE DO IF INCHRW="X" THEN DONE;
END "TEST";